/*
 #
 #  Files       : SymMatrix.h
 #                ( C++ header file )
 #
 #  Description : The SmallMatrix Library
 #                ( http://code.google.com/p/smallmatrix )
 #
 #  Copyright   : Olivier Juan
 #                ( http://www.mas.ecp.fr/vision/Personnel/juan/ )
 #
 #  License     : CeCILL-C
 #                ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #  This software is governed by the CeCILL-C license under French law and
 #  abiding by the rules of distribution of free software.  You can  use,
 #  modify and or redistribute the software under the terms of the CeCILL-C
 #  license as circulated by CEA, CNRS and INRIA at the following URL
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL-C license and that you accept its terms.
 #
*/

#ifndef _SYMMATRIX_H
#define _SYMMATRIX_H

#if USE_MKL
#include <mkl.h>
#elif USE_ACML
#include <acml.h>
#endif

#include <SmallMatrix/SmallMatrix.h>

NAMESPACE_BEGIN(SmallMatrix)

template <int N> class SymMatrix : public Matrix_Base<SymMatrix<N> >
{
private:
	typedef Matrix_Base<SymMatrix<N> > Base;
public:
	template <int IdxR, int IdxC>
		reel& Access() {
			STATIC_ASSERT((IdxR<N)&&(IdxR>=0));
			STATIC_ASSERT((IdxC<N)&&(IdxC>=0));
			enum { RowOrd = IdxR < IdxC ? IdxR : IdxC };
			enum { ColOrd = IdxR < IdxC ? IdxC : IdxR };
			enum { IdxAcc = ((RowOrd * ((N*2)-RowOrd-1))>>1)+ColOrd };
			return Base::template TabAccess<IdxAcc>();
		}
	template <int IdxR, int IdxC>
		const reel& Access() const {
			STATIC_ASSERT((IdxR<N)&&(IdxR>=0));
			STATIC_ASSERT((IdxC<N)&&(IdxC>=0));
			enum { RowOrd = IdxR < IdxC ? IdxR : IdxC };
			enum { ColOrd = IdxR < IdxC ? IdxC : IdxR };
			enum { IdxAcc = ((RowOrd * ((N*2)-RowOrd-1))>>1)+ColOrd };
			return Base::template TabAccess<IdxAcc>();
		}
public:
	INLINE SymMatrix() : Base(){}
	INLINE SymMatrix(const reel &v) : Base(v){}
	INLINE SymMatrix(const reel tab[Base::SIZE]) : Base(tab){}
	INLINE SymMatrix(const Vecteur<N>& v) {
		SymMatrix<N>& ref=*this;
		const Matrix<1,N>& right = *reinterpret_cast<const Matrix<1,N>*>(&v);
		matrix_product_Unrolled<N>::template Operation<SymMatrix<N>,Vecteur<N>,Matrix<1,N> >::compute(ref,v,right);
	}
	template <typename TT> INLINE SymMatrix(const Matrix_Base<TT>& copy) : Base(copy){}
	template <typename TT> INLINE SymMatrix<N>& operator=(const Matrix_Base<TT>& copy) {
		Base& ref=*this; 
		return ref = copy;
	}
	INLINE SymMatrix<N>& operator=(const SymMatrix<N>& copy) {
		Base& ref=*this;
		return ref = copy;
	}
	INLINE SymMatrix(const SymMatrix<N>& copy) : Base(copy){}
	INLINE reel& operator()(const int idxm,const int idxn);
	INLINE const reel& operator()(const int idxm,const int idxn)const;
	//template <typename T1,typename T2> INLINE SymMatrix<N>& prod(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1,typename T2> INLINE SymMatrix<N>& add(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1,typename T2> INLINE SymMatrix<N>& subst(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2);
	//template <typename T1> INLINE SymMatrix<N>& add(const Matrix_Base<T1>& M1);
	//template <typename T1> INLINE SymMatrix<N>& subst(const Matrix_Base<T1>& M1);
	//template <typename TT> INLINE SymMatrix<N>& mult(const Matrix_Base<TT>& Two);
	//template <typename TT> INLINE SymMatrix<N>& div(const Matrix_Base<TT>& Two);
	INLINE reel trace()const;
	INLINE SymMatrix<N> inv()const;
	INLINE SymMatrix<N> inv(reel& deter)const;
	INLINE reel det()const;
#if USE_MKL || USE_ACML
	INLINE SymMatrix<N> pinv2(double reltol); // attention, modifie *this !!
	INLINE SymMatrix<N> pinv(double reltol)const {SymMatrix<N> a(*this); return a.pinv2(reltol);}
#endif
	INLINE SymMatrix<N>& addDiag(const reel& x);
	INLINE SymMatrix<N> tred2(Vecteur<N>& d,Vecteur<N>& e);
	INLINE SquareMatrix<N> tqli(Vecteur<N>& d, Vecteur<N>& e);
	INLINE SquareMatrix<N> eigenVV(Vecteur<N>& eigenValues) const;
};
template <int N> INLINE reel& SymMatrix<N>::operator()(const int idxm,const int idxn)
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	SymMatrix<N>& ref=*this;
	if (idxm<=idxn)
		return ref[((idxm*((N<<1)-idxm-1))>>1)+idxn];
	else
		return ref[((idxn*((N<<1)-idxn-1))>>1)+idxm];
}
template <int N> INLINE const reel& SymMatrix<N>::operator()(const int idxm,const int idxn) const
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	const SymMatrix<N>& ref=*this;
	if (idxm<=idxn)
		return ref[((idxm*((N<<1)-idxm-1))>>1)+idxn];
	else
		return ref[((idxn*((N<<1)-idxn-1))>>1)+idxm];
}
//template <int N> template <typename T1,typename T2> INLINE SymMatrix<N>& SymMatrix<N>::prod(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++) {
//			ref(i,j)=0;
//			for (int k=0;k<MN;k++)
//				ref(i,j)+=M1(i,k)*M2(k,j);
//		}
//		return ref;
//}
//template <int N> template <typename T1,typename T2> INLINE SymMatrix<N>& SymMatrix<N>::add(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++)
//			ref(i,j)=M1(i,j)+M2(i,j);
//	return ref;
//}
//
//template <int N> template <typename T1,typename T2> INLINE SymMatrix<N>& SymMatrix<N>::subst(const Matrix_Base<T1>& M1,const Matrix_Base<T2>& M2) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++)
//			ref(i,j)=M1(i,j)-M2(i,j);
//	return ref;
//}
//template <int N> template <typename T1> INLINE SymMatrix<N>& SymMatrix<N>::add(const Matrix_Base<T1>& M1) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++)
//			ref(i,j)+=M1(i,j);
//	return ref;
//}
//
//template <int N> template <typename T1> INLINE SymMatrix<N>& SymMatrix<N>::subst(const Matrix_Base<T1>& M1) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++)
//			ref(i,j)-=M1(i,j);
//	return ref;
//}
//template <int N> template <typename TT> INLINE SymMatrix<N>& SymMatrix<N>::mult(const Matrix_Base<TT>& Two) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++)
//			ref(i,j)*=Two(i,j);
//	return ref;
//}
//template <int N> template <typename TT> INLINE SymMatrix<N>& SymMatrix<N>::div(const Matrix_Base<TT>& Two) {
//	SymMatrix<N>& ref=*this;
//	for (int i=0;i<N;i++)
//		for (int j=i;j<N;j++) {
//			assert(Two(i,j)!=0);
//			ref(i,j)/=Two(i,j);
//		}
//	return ref;
//}

template <int N> INLINE reel SymMatrix<N>::trace()const
{
	const SymMatrix<N>& ref=*this;
	return trace_Unrolled<N>::compute(ref);
}

template <int N> INLINE	SymMatrix<N> SymMatrix<N>::inv() const {
	SymMatrix<N> res;
	reel deter = det();
	assert(deter != 0);
	const SymMatrix<N>& ref = *this;
	inverse_Unrolled<N>::compute(res,ref,deter);
	return res;
}
template <int N> INLINE	SymMatrix<N> SymMatrix<N>::inv(reel& deter) const {
	SymMatrix<N> res;
	deter = det();
	assert(deter != 0);
	const SymMatrix<N>& ref = *this;
	inverse_Unrolled<N>::compute(res,ref,deter);
	return res;
}
template <int N> INLINE reel SymMatrix<N>::det() const {
	const SymMatrix<N>& ref = *this;
	return det_Unrolled<IndexListEnd,N,N,N>::compute(ref);
}

#if USE_MKL || USE_ACML
template<int N> INLINE SymMatrix<N> SymMatrix<N>::pinv2(double reltol) {
	// svd
	reel evalues[N];
	SquareMatrix<N> evectors;
	reel work[3*N];
	int info;
	int n=N;
#if USE_MKL
	dspev("V","L",&n,Tab,evalues,&evectors(0,0),&n,work,&info);
#elif USE_ACML
	dspev_("V","L",&n,Tab,evalues,&evectors(0,0),&n,work,&info);
#endif
	// result = evectors' * pinv(diag(evalues)) * evectors
	SquareMatrix<N> left;
	double tol=max(-evalues[0],evalues[N-1])*reltol;
	for (int i=0; i<N; i++) {
		double d = evalues[i];
		if (d>=tol) for (int j=0; j<N; j++) left(j,i)=evectors(i,j)/d;
		else for (int j=0; j<N; j++) left(i,j)=0;
	}
	SymMatrix<N> res;
	res.prod(left,evectors);
	return res;
}
#endif

template <int N> INLINE SymMatrix<N>& SymMatrix<N>::addDiag(const reel& x)
{
	SymMatrix<N>& ref = *this;
	addDiag_Unrolled<N>::compute(ref,x);
	return ref;
}

template <int N> INLINE SymMatrix<N> SymMatrix<N>::tred2(Vecteur<N>& d,Vecteur<N>& e)
{
	int l,k,j,i;
	reel scale,hh,h,g,f;
	SymMatrix<N>& local = *this;
	SquareMatrix<N> a;
	for (i=0;i<N;i++)
	{
		a(i,i)=local(i,i);
		for (int j=0;j<i;j++)
		{
			a(i,j)=a(j,i)=local(i,j);
		}
	}
	for (i=N-1;i>=1;i--) {
		l=i-1;
		h=scale=0.0;
		if (l > 0) {
			for (k=0;k<=l;k++)
				scale += fabs(a(i,k));
			if (scale == 0.0)
				e(i)=a(i,l);
			else {
				for (k=0;k<=l;k++) {
					a(i,k) /= scale;
					h += a(i,k)*a(i,k);
				}
				f=a(i,l);
				g=(f >= 0.0 ? -::sqrt(h) : ::sqrt(h));
				e(i)=scale*g;
				h -= f*g;
				a(i,l)=f-g;
				f=0.0;
				for (j=0;j<=l;j++) {
					a(j,i)=a(i,j)/h;
					g=0.0;
					for (k=0;k<=j;k++)
						g += a(j,k)*a(i,k);
					for (k=j+1;k<=l;k++)
						g += a(k,j)*a(i,k);
					e(j)=g/h;
					f += e(j)*a(i,j);
				}
				hh=f/(h+h);
				for (j=0;j<=l;j++) {
					f=a(i,j);
					e(j)=g=e(j)-hh*f;
					for (k=0;k<=j;k++)
						a(j,k) -= (f*e(k)+g*a(i,k));
				}
			}
		} else
			e(i)=a(i,l);
		d(i)=h;
	}
	d(0)=0.0;
	e(0)=0.0;
	// Contents of this loop can be omitted if eigenvectors not
	//		wanted except for statement d(i)=a(i,i); 
	for (i=0;i<=N-1;i++) {
		l=i-1;
		if (d(i)) {
			for (j=0;j<=l;j++) {
				g=0.0;
				for (k=0;k<=l;k++)
					g += a(i,k)*a(k,j);
				for (k=0;k<=l;k++)
					a(k,j) -= g*a(k,i);
			}
		}
		d(i)=a(i,i);
		a(i,i)=1.0;
		for (j=0;j<=l;j++) a(j,i)=a(i,j)=0.0;
	}
	for (i=0;i<N;i++)
	{
		local(i,i)=a(i,i);
		for (int j=0;j<i;j++)
		{
			local(i,j)=a(i,j);
		}
	}
	return local;
}

template <int N> INLINE SquareMatrix<N> SymMatrix<N>::tqli(Vecteur<N>& d, Vecteur<N>& e)
{
	int m,l,iter,i,k;
	reel s,r,p,g,f,dd,c,b;
	SquareMatrix<N> res;
	SymMatrix<N>& local = *this;
	//recopie de this dans res
	for (i=0;i<N;i++)
	{
		res(i,i)=local(i,i);
		for (int j=0;j<i;j++)
		{
			res(i,j)=local(i,j);
			res(j,i)=local(i,j);
		}
	}

	for (i=1;i<N;i++) e[i-1]=e[i];
	e[N-1]=0.0;
	for (l=0;l<N;l++) {
		iter=0;
		do {
			for (m=l;m<N-1;m++) {
				dd=fabs(d[m])+fabs(d[m+1]);
				if (fabs(e[m])+dd == dd) break;
			}
			if (m != l) {
				if (iter++ == 30) { std::cout<<"Too many iterations in tqli"<<std::endl; return res; }
				g=(d[l+1]-d[l])/(reel(2.0)*e[l]);
				r=pythag(g,1.0);
				g=d[m]-d[l]+e[l]/(g+(g >= 0 ? (r >= 0 ? r : -r) : (r >= 0 ? -r : r)));
				s=c=1.0;
				p=0.0;
				for (i=m-1;i>=l;i--) {
					f=s*e[i];
					b=c*e[i];
					e[i+1]=(r=pythag(f,g));
					if (r == 0.0) {
						d[i+1] -= p;
						e[m]=0.0;
						break;
					}
					s=f/r;
					c=g/r;
					g=d[i+1]-p;
					r=(d[i]-g)*s+reel(2.0)*c*b;
					d[i+1]=g+(p=s*r);
					g=c*r-b;
					// Next loop can be omitted if eigenvectors not wanted
					for (k=0;k<N;k++) {
						f=res(k,i+1);
						res(k,i+1)=s*res(k,i)+c*f;
						res(k,i)=c*res(k,i)-s*f;
					}
				}
				if (r == 0.0 && i >= l) continue;
				d[l] -= p;
				e[l]=g;
				e[m]=0.0;
			}
		} while (m != l);
	}
	return res;
}


template <int N> INLINE SquareMatrix<N> SymMatrix<N>::eigenVV(Vecteur<N>& eigenValues)const
{
	Vecteur<N> temp;
	//glauque
	SymMatrix<N> tempo(*this);
	return tempo.tred2(eigenValues,temp).tqli(eigenValues,temp);
}
NAMESPACE_END

#endif
